
#include <stdlib.h>
#include <stdint.h>

#include "../riscv_debug.h"
#include "../target.h"
#include "../jtag.h"
#include "flash.h"
#include "algo.h"

extern target_t target_riscv;

static int target_load_algorithm(unsigned int algo_adr,
                                 unsigned char *algo_buf,
                                 unsigned int algo_sz)
{
    if (!target_riscv.write_mem(algo_adr, algo_buf, algo_sz))
        return 0;
    else
        return -1;
}

static int target_run_algorithm(unsigned int algo_fun,
                                unsigned int ret_adr,
                                unsigned int *ret_stat)
{
    if (target_riscv.write_reg(&algo_fun, 32, 4))
    {
        asm volatile("ebreak");
        return -1;
    }

    if (target_riscv.resume(NULL, 0))
    {
        asm volatile("ebreak");
        return -1;
    }

    for (volatile int j = 0; j < 4096; j++)
    {
        for (volatile int i = 0; i < 1024; i++)
            asm volatile("nop");

        int rst = target_riscv.halt_reason();
        //asm volatile("ebreak");
        if (rst == 4) // ebreak;
        {
            uint32_t A0 = (uint32_t)-1;
            uint32_t PC = (uint32_t)-1;
            if (!target_riscv.read_reg(&A0, 10, 4))
                *ret_stat = A0; // 远程函数调用返回值
            else
            {
                asm volatile("ebreak");
                return -1;
            }

            if (!target_riscv.read_reg(&PC, 32, 4))
            {
                // asm volatile("ebreak");
                if (PC == ret_adr)
                    return 0;
                else
                {
                    asm volatile("ebreak");
                    return -1;
                }
            }
            else
            {
                asm volatile("ebreak");
                return -1;
            }
        }
    }

    asm volatile("ebreak");
    return -1;
}

int flash_init(void)
{
    int rst;
//    uint32_t PC = -1;

    rst = target_load_algorithm(algo[0].algo->load_address,
                                algo[0].algo->instructions,
                                algo[0].algo->algo_sz);

    //asm volatile("ebreak");
    if (rst)
        return -1;
        
    unsigned int stat;
    unsigned int sp = algo[0].algo->begin_stack;
    unsigned int ra = algo[0].algo->load_address;
    target_riscv.write_reg(&sp, 2, 4);
    target_riscv.write_reg(&ra, 1, 4);
    rst = target_run_algorithm(algo[0].algo->pc_Init,
                               algo[0].algo->load_address,
                               &stat);

//    target_riscv.halt();
//    riscv_debug_halt_check(&stat);
//    target_riscv.read_reg(&PC, 32, 4);
    //asm volatile("ebreak");

    if (!rst)
        return 0;
    else
        return -1;
}

int flash_erase(unsigned int adr, unsigned int sz)
{
    int rst;       
    unsigned int stat;
    unsigned int sp = algo[0].algo->begin_stack;
    unsigned int a0 = adr;
    unsigned int a1 = sz;
    unsigned int ra = algo[0].algo->load_address;
    target_riscv.write_reg(&sp, 2, 4);
    target_riscv.write_reg(&a0, 10, 4);
    target_riscv.write_reg(&a1, 11, 4);
    target_riscv.write_reg(&ra, 1, 4);
    rst = target_run_algorithm(algo[0].algo->pc_EraseSector,
                               algo[0].algo->load_address,
                               &stat);

    //asm volatile("ebreak");
    if (!rst)
        return 0;
    else
        return -1;
}

int flash_program(unsigned int adr, unsigned char *buf, unsigned int sz)
{
    int rst;
    unsigned int stat;
    unsigned int sp;
    unsigned int a0;
    unsigned int a1;
    unsigned int a2;
    unsigned int ra;
    unsigned int f_adr = adr;
    unsigned char *f_buf = buf;
    unsigned int f_sec_sz = algo[0].algo->sector[0].sector_sz / 4; // 4096 / 4
    unsigned int left = sz;
    // unsigned int blk_num = 0;

    while (left >= f_sec_sz)
    {
        target_riscv.write_mem(algo[0].algo->begin_data, f_buf, f_sec_sz);

        sp = algo[0].algo->begin_stack;
        a0 = f_adr;
        a1 = algo[0].algo->begin_data;
        a2 = f_sec_sz;
        ra = algo[0].algo->load_address;
        target_riscv.write_reg(&sp, 2, 4);
        target_riscv.write_reg(&a0, 10, 4);
        target_riscv.write_reg(&a1, 11, 4);
        target_riscv.write_reg(&a2, 12, 4);
        target_riscv.write_reg(&ra, 1, 4);
        rst = target_run_algorithm(algo[0].algo->pc_ProgramPage,
                                   algo[0].algo->load_address,
                                   &stat);
        // asm volatile("ebreak");
        if (rst)
            return -1;

        // blk_num++;
        left -= f_sec_sz;
        f_buf += f_sec_sz;
        f_adr += f_sec_sz;
    }

    if (left)
    {
        target_riscv.write_mem(algo[0].algo->begin_data, f_buf, left);

        sp = algo[0].algo->begin_stack;
        a0 = f_adr;
        a1 = algo[0].algo->begin_data;
        a2 = left;
        ra = algo[0].algo->load_address;
        target_riscv.write_reg(&sp, 2, 4);
        target_riscv.write_reg(&a0, 10, 4);
        target_riscv.write_reg(&a1, 11, 4);
        target_riscv.write_reg(&a2, 12, 4);
        target_riscv.write_reg(&ra, 1, 4);
        rst = target_run_algorithm(algo[0].algo->pc_ProgramPage,
                                   algo[0].algo->load_address,
                                   &stat);
        //asm volatile("ebreak");
        if (rst)
            return -1;
    }

    return 0;
}

const unsigned char crc8_d_program[] = {
#include "crc8_d.inc"
};

int flash_crc8_d(unsigned int loadadr, unsigned int fun, unsigned int adr, unsigned int sz, unsigned int crc)
{
    int rst;
    
    RISCV_DEBUG_LOG_DBG("adr %x, sz %x \n", adr, sz);

    rst = target_load_algorithm(loadadr,
                                crc8_d_program,
                                sizeof(crc8_d_program));

    if (rst)
        return -1;
        
    unsigned int stat;
    unsigned int sp = (loadadr / 4 + 1) * 4 + 1024; // 上取整
    unsigned int a0 = adr;
    unsigned int a1 = sz;
    unsigned int ra = loadadr;
    target_riscv.write_reg(&sp, 2, 4);
    target_riscv.write_reg(&a0, 10, 4);
    target_riscv.write_reg(&a1, 11, 4);
    target_riscv.write_reg(&ra, 1, 4);
    rst = target_run_algorithm(loadadr + fun,   // fun addr
                               loadadr,         // rst addr
                               &stat);

    RISCV_DEBUG_LOG_DBG("crc8 %x \n", stat);

    if (!rst && (crc == stat))
        return 0;
    else
        return -1;
}
